#!/usr/bin/env python3
# -*- coding:UTF-8 -*-

import rospy,sys,cv2,time
import math
import rospy  
from geometry_msgs.msg import Twist
import time  
from std_msgs.msg import String
from std_msgs.msg import Float64

a=0
b=0
begin=0
lift=0
t=0
pub1 = rospy.Publisher('arm_back',String,queue_size=10)
pub = rospy.Publisher('arm_back',String,queue_size=10)

def move_chassis(linear_speed, angular_speed, duration): 
	# 初始化ROS节点  
		#rospy.init_node('move_chassis_node', anonymous=True)   
	# 创建发布者，发布到/cmd_vel话题（这通常是底盘驱动节点的订阅话题）  
		pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)   
	# 创建Twist消息  
		move_cmd = Twist()  
	# 设置速度和角速度  
		move_cmd.linear.x = linear_speed  
		move_cmd.angular.z = angular_speed   
	    # 开始移动底盘  
		start_time = time.time()  
		while (time.time() - start_time) < duration:  
		# 发布速度指令  
			pub.publish(move_cmd)   
	    # 停止底盘  
		move_cmd.linear.x = 0  
		move_cmd.angular.z = 0  
		pub.publish(move_cmd)    
	    # 等待一段时间确保底盘停下来  
		time.sleep(1)

class Loop:
	def  __init__(self):
		rospy.on_shutdown(self.cleanup)
		self.joint1=rospy.Publisher('/tilt_controller/command',Float64,queue_size=10)
		self.joint2=rospy.Publisher('/shoulder_controller/command',Float64,queue_size=10)
		self.joint3=rospy.Publisher('/elbow_controller/command',Float64,queue_size=10)
		self.joint4=rospy.Publisher('/wrist_controller/command',Float64,queue_size=10)
		self.joint5=rospy.Publisher('/hand_controller/command',Float64,queue_size=10)

		#物体相对于机械臂基座坐标系的位置
		self.x= 0.1
		self.y= 0
		self.z= 0.
		#机械臂参数初始化
		self.armsub=0
		self.a2=0.105
		self.a3=0.105
		self.a4=0.12
		self.arm_length=0.3
		#Define five poses
		self.pos1 = Float64()
		self.pos2 = Float64()
		self.pos3 = Float64()
		self.pos4 = Float64()
		self.pos5 = Float64()
		self.bag="right"
		#self.grasp()        	
		#self.lift()

		while not rospy.is_shutdown():
			self.listen()

	def listen(self):
		rospy.Subscriber("arm",String,self.mgrasp)
		rospy.Subscriber("choose_bag",String,self.choose_bag)
	


	def mgrasp(self,data):
		global begin
		global lift
		if data.data=="grasp":	
			global a
			if a==0:
				self.grasp()
				a=1
		elif data.data=="give":	
			global b
			if b==0 and a==1:
				self.give()
				a=0
				b=1
	
	def choose_bag(self,data):
		if data.data=="left" or data.data == "right":
			self.bag=data.data
	
    
	def grasp(self):
		print("grasp")
		
		self.joint2.publish(3.6968936)
		rospy.sleep(0.5)
		self.joint4.publish(4.310486013)
		rospy.sleep(0.5)
		self.joint3.publish(1.1811652)
		rospy.sleep(0.5)	
		self.joint5.publish(3.175340230)
		
		rospy.sleep(1.0)
		self.joint1.publish(4.111068)
		rospy.sleep(5)
		move_chassis(0.07,0,5)
		rospy.sleep(5)
		#位置1
		
		self.joint5.publish(2)
		rospy.sleep(0.5)
		self.joint3.publish(0.7874434711)
		rospy.sleep(0.5)	
		self.joint2.publish(4.8831721747)
		rospy.sleep(0.5)
		self.joint4.publish(4.27980639)
		rospy.sleep(3.0)
		self.joint1.publish(4.1212950)
		rospy.sleep(3.0)
		
		
		pub1.publish("grasp_finish")
	
	def give(self):
	#伸出
		#位置1
		self.joint2.publish(3.6968936)
		rospy.sleep(0.5)
		self.joint4.publish(4.310486013)
		rospy.sleep(0.5)
		self.joint3.publish(1.1811652)
		rospy.sleep(0.5)	
		self.joint5.publish(3.175340230)
		rospy.sleep(1.0)
		self.joint1.publish(4.111068)
		rospy.sleep(5)
		#位置2
		self.joint5.publish(1.994175024)
		rospy.sleep(0.5)
		self.joint3.publish(0.7874434711)
		rospy.sleep(0.5)	
		self.joint2.publish(4.8831721747)
		rospy.sleep(0.5)
		self.joint4.publish(4.27980639)
		rospy.sleep(3.0)
		self.joint1.publish(4.1212950)
		rospy.sleep(3.0)
		pub = rospy.Publisher('arm_back',String,queue_size=10)
		pub.publish("hand_finish")
		
	def cleanup(self):
		rospy.loginfo("Shutting down turtlebot arm ....")

if __name__ =="__main__":
	rospy.init_node('arm_grasp',anonymous=True)
	loop=Loop()
	rospy.spin()
	#loop.grasp()
    #loop.init_arm()
    #loop.grasp()
    #rospy.sleep(5)
    #loop.lift()
    #rospy.sleep(5)

    #loop.put()
    #rospy.sleep(5)

